//
// Created by hrg on 2019/12/5.
//
#include <ecxml.h>
#include <iostream>
#include <cstring>
#include <cassert>
#include <cmath>

/**
 * 字符串转换成16进制数据
 * */
long atox(const char* ptr){
    assert(ptr != NULL);
    long sum = 0;

    int A = 0;

    int len = strlen(ptr);
    for (int i = len - 1; i >= 0; --i) {
        if(ptr[i] >= 'a' && ptr[i] <= 'f'){
            A = ptr[i] - 'a' + 10;
        } else if(ptr[i] >= 'A' && ptr[i] <= 'F'){
            A = ptr[i] - 'A' + 10;
        } else {
            A = ptr[i] - 48;
        }
        sum += A * pow(16.0, len - i - 1);
    }
    return sum;
}

EcXml::EcXml(): m_File_(NULL), m_Tree_(NULL){
    m_Slot_ = 0;
    m_PdoIncrement_ = 0;
    m_IndexIncrement_ = 0;
}

EcXml::~EcXml(){
    close();
}

bool EcXml::CreatePdo(mxml_node_t* node, ec_pdo_info_t* pdo_info){
    assert(node != NULL && pdo_info != NULL);

    int n_entries = 0;
    int num = -1;
    mxml_node_t* temp = NULL;
    mxml_node_t* entry= NULL;
    mxml_node_t* index= NULL;
    ec_pdo_entry_info_t* entryInfo = NULL;

    temp = mxmlFindElement(node, node, "Index", NULL, NULL, MXML_DESCEND);
    if(temp == NULL){
        return false;
    }

    // 判断pdo是否对slot有依赖
    const char* PdoSlot = mxmlElementGetAttr(temp, "DependOnSlot");
    if(m_Slot_ && PdoSlot && !strcmp(PdoSlot, "1")){
        // pdo_info index
        pdo_info->index = atox(temp->child->value.text.string + 2) + m_PdoIncrement_ * m_Slot_ ;
    } else {
        pdo_info->index = atox(temp->child->value.text.string + 2);
    }

    temp = mxmlFindElement(node, node, "Entry", NULL, NULL, MXML_DESCEND);
    while(1) {
        if (temp == NULL)
            break;

        n_entries += 1;
        temp = mxmlFindElement(temp, node, "Entry", NULL, NULL, MXML_DESCEND);
    }

    pdo_info->n_entries = n_entries;
    pdo_info->entries = (ec_pdo_entry_info_t*)malloc(sizeof(ec_pdo_entry_info_t) * n_entries);
    memset(pdo_info->entries, 0, sizeof(ec_pdo_entry_info_t) * n_entries);

    temp = mxmlFindElement(node, node, "Entry", NULL, NULL, MXML_DESCEND);
    while(1) {
        if (temp == NULL)
            break;

        entryInfo = &(pdo_info->entries[++num]);

        index = mxmlFindElement(temp, temp, "Index", NULL, NULL, MXML_DESCEND);
        const char* IndexSlot = mxmlElementGetAttr(index, "DependOnSlot");
        if(m_Slot_ && IndexSlot && !strcmp(IndexSlot, "1")){
            // pdo_info index
            entryInfo->index = (uint16_t)atox(index->child->value.text.string + 2) + m_IndexIncrement_ * m_Slot_ ;
        } else {
            entryInfo->index = (uint16_t)atox(index->child->value.text.string + 2);
        }
        
        entry = mxmlFindElement(temp, temp, "SubIndex", NULL, NULL, MXML_DESCEND);
        if (entry != nullptr){
        	entryInfo->subindex = (uint8_t)atoi(entry->child->value.text.string);
		}
        entry = mxmlFindElement(temp, temp, "BitLen", NULL, NULL, MXML_DESCEND);
        if (entry != nullptr){
        	entryInfo->bit_length = (uint8_t)atoi(entry->child->value.text.string);
        }

        temp = mxmlFindElement(temp, node, "Entry", NULL, NULL, MXML_DESCEND);
    }
    return true;
}

bool EcXml::CreatePdo(mxml_node_t* node, const char* element, ec_sync_info_t* syncInfo){
    assert(syncInfo != NULL);

    int num = 0;
    int pdonum = 0;

    mxml_node_t *temp = NULL;
    temp = mxmlFindElement(node, node, element, NULL, NULL, MXML_DESCEND);
    if(temp == NULL){
        if(element == "RxPdo") {
            syncInfo->index = 0;
            syncInfo->dir = EC_DIR_OUTPUT;
            syncInfo->watchdog_mode = EC_WD_ENABLE;
        } else {
            syncInfo->index = 1;
            syncInfo->dir = EC_DIR_INPUT;
            syncInfo->watchdog_mode = EC_WD_DISABLE;
        }
        return false;
    }

    const char* pdosm = mxmlElementGetAttr(temp, "Sm");
    if(pdosm == NULL){
        return false;
    }

    for (temp = mxmlFindElement(node, node, element, NULL, NULL, MXML_DESCEND);
         temp != NULL;
         temp = mxmlFindElement(temp, node, element, NULL, NULL, MXML_DESCEND)) {
        if(!mxmlElementGetAttr(temp, "Sm"))
            continue;
        ++ pdonum;
    }

    ec_pdo_info_t *tempPdoInfo = new ec_pdo_info_t[pdonum];
    for (temp = mxmlFindElement(node, node, element, NULL, NULL, MXML_DESCEND);
         temp != NULL;
         temp = mxmlFindElement(temp, node, element, NULL, NULL, MXML_DESCEND)) {
        if(!mxmlElementGetAttr(temp, "Sm"))
            continue;
        
        CreatePdo(temp, &tempPdoInfo[num]);
        ++ num;
    }

    if(num != 0) {
        if(element == "RxPdo") {
            syncInfo->index = strtoul(pdosm, NULL, 16);
            syncInfo->dir = EC_DIR_OUTPUT;
            syncInfo->watchdog_mode = EC_WD_ENABLE;
        } else {
            syncInfo->index = strtoul(pdosm, NULL, 16);
            syncInfo->dir = EC_DIR_INPUT;
            syncInfo->watchdog_mode = EC_WD_DISABLE;
        }
        syncInfo->n_pdos = pdonum;
        syncInfo->pdos = tempPdoInfo;
    }
    return true;
}

bool EcXml::open(const char* filename){
    m_File_ = fopen(filename, "r");
    if(m_File_ == NULL){
        return false;
    }

    m_Tree_ = mxmlLoadFile(NULL, m_File_, MXML_TEXT_CALLBACK);
    if(m_Tree_ == NULL){
        fclose(m_File_);
        return false;
    }
    return true;
}

const char* EcXml::ProfileNo(){
    mxml_node_t* node = mxmlFindElement(m_Tree_, m_Tree_, "EtherCATInfo", NULL, NULL, MXML_DESCEND);
    if(node == NULL)
        return NULL;

    mxml_node_t* Profile = mxmlFindElement(node, node, "Profile", NULL, NULL, MXML_DESCEND);
    if(Profile == NULL){
        return NULL;
    }

    mxml_node_t* ProfileNo = mxmlFindElement(Profile, Profile, "ProfileNo", NULL, NULL, MXML_DESCEND);
    if(ProfileNo == NULL){
        return NULL;
    }
    
    return mxmlGetText(ProfileNo, NULL);
}

EcType EcXml::GetType(){
    mxml_node_t* node = mxmlFindElement(m_Tree_, m_Tree_, "EtherCATInfo", NULL, NULL, MXML_DESCEND);
    if(node == NULL)
        return EC_OTHER;

    mxml_node_t* Profile = mxmlFindElement(node, node, "Profile", NULL, NULL, MXML_DESCEND);
    if(Profile == NULL){
        return EC_SERVO;
    }

    mxml_node_t* ProfileNo = mxmlFindElement(Profile, Profile, "ProfileNo", NULL, NULL, MXML_DESCEND);
    if(ProfileNo == NULL){
        return EC_SERVO;
    }

    if(!strcmp(mxmlGetText(ProfileNo, NULL), "402")){
        return EC_SERVO;
    }
    
    return EC_IO; // IO
}

bool EcXml::IsModule(ec_slave_info_t slvinfo){
    if(m_EtherCATInfo_ == NULL){
        return false;
    }

    mxml_node_t* modules = mxmlFindElement(m_EtherCATInfo_, m_EtherCATInfo_, "Modules", NULL, NULL, MXML_DESCEND);
    if(modules == NULL){
        return false;
    } else {
        return true;
    }
    return false;    
}

bool EcXml::match(ec_slave_info_t slvinfo) {
    if(m_Tree_ == NULL){
        return false;
    }
    mxml_node_t* node = m_Tree_;
    do{
        node = mxmlFindElement(node, m_Tree_, "EtherCATInfo", NULL, NULL, MXML_DESCEND);
        if(node == NULL)
            break;

        mxml_node_t* vendor = mxmlFindElement(node, node, "Vendor", NULL, NULL, MXML_DESCEND);
        if(vendor == NULL)
            break;

        mxml_node_t* Id = mxmlFindElement(vendor, vendor, "Id", NULL, NULL, MXML_DESCEND);
        if(Id == NULL)
            break;

        uint32_t vid = 0;
        if(Id->child->value.text.string[0] == '#')
            vid = atox(Id->child->value.text.string + 2);
        else
            vid = atoi(Id->child->value.text.string);
        
        if(vid != slvinfo.vendor_id)
            continue;

        mxml_node_t* devices = mxmlFindElement(node, node, "Devices", NULL, NULL, MXML_DESCEND);
        if(devices == NULL)
            continue;

        // find product_code
        for(mxml_node_t* device = devices; device != NULL; ){
            device = mxmlFindElement(device, devices, "Device", NULL, NULL, MXML_DESCEND);
            if(device == NULL)
                break;
            /* 2020-5-6 21:16 */
            mxml_node_t* type = mxmlFindElement(device, device, "Type", NULL, NULL, MXML_DESCEND);
            if(type == NULL)
                continue;

            const char* ProductCode = mxmlElementGetAttr(type, "ProductCode");
            if(ProductCode[0] == '#'){
                // hex(std::cout);
                if(atox(ProductCode + 2) != slvinfo.product_code){
                    continue;
                }
            } else {
                if(atoi(ProductCode) != slvinfo.product_code){
                    continue;
                }
            }
            const char* RevisionNo = mxmlElementGetAttr(type, "RevisionNo");
            if(RevisionNo[0] == '#'){
                if(atox(RevisionNo + 2) != slvinfo.revision_number){
                    continue;
                }
            } else {
                if(atoi(RevisionNo) != slvinfo.revision_number){
                    continue;
                }
            }

            mxml_node_t* modules = mxmlFindElement(node, node, "Modules", NULL, NULL, MXML_DESCEND);
            if(modules == NULL){
                m_EtherCATInfo_ = device;
            } else {
                m_EtherCATInfo_ = node;
            }
            return true;
        }
    } while(true);
    return false;
}

void EcXml::close(){
    if(m_Tree_ != NULL){
        mxmlRelease(m_Tree_);
        m_Tree_ = NULL;
    }

    if(m_File_ != NULL){
        fclose(m_File_);
        m_File_ = NULL;
    }
}

bool EcXml::GetModuleSyncInfo(ec_sync_info_t **syncInfo, const char *ModuleIdent, int slot){
    m_Slot_ = slot;
    if(*syncInfo == NULL){
        ec_sync_info_t* syncInfoTemp = new ec_sync_info_t[3];
        syncInfoTemp[0].n_pdos = 0;
        syncInfoTemp[0].dir = EC_DIR_INVALID;
        syncInfoTemp[0].pdos = NULL;
        syncInfoTemp[1].n_pdos = 0;
        syncInfoTemp[1].dir = EC_DIR_INVALID;
        syncInfoTemp[1].pdos = NULL;

        syncInfoTemp[2].index = (uint8_t)EC_END;
        *syncInfo = syncInfoTemp;

        // 获取Slots
        mxml_node_t* Slots = mxmlFindElement(m_EtherCATInfo_, m_EtherCATInfo_, "Slots", NULL, NULL, MXML_DESCEND);
        if(Slots != NULL){
            m_PdoIncrement_ = atoi(mxmlElementGetAttr(Slots, "SlotGroupPdoIncrement"));
            m_IndexIncrement_ = atoi(mxmlElementGetAttr(Slots, "SlotGroupIndexIncrement"));
        }
    }
    mxml_node_t* modules = mxmlFindElement(m_EtherCATInfo_, m_EtherCATInfo_, "Modules", NULL, NULL, MXML_DESCEND);
    if(modules == NULL){
        return false;
    }

    mxml_node_t* module = modules;
    while(1){
        module = mxmlFindElement(module, modules, "Module", NULL, NULL, MXML_DESCEND);
        if(module == NULL) {
            return false;
        }

        if(mxmlFindElement(module, module, "Type", "ModuleIdent", ModuleIdent, MXML_DESCEND)){
            if ((&(*syncInfo)[0])->n_pdos == 0){
                CreatePdo(module, "RxPdo", &(*syncInfo)[0]);
            }

            if ((&(*syncInfo)[1])->n_pdos == 0){
                CreatePdo(module, "TxPdo", &(*syncInfo)[1]);
            }
            break;
        }
    }
    return true;
}

bool EcXml::GetServoSyncInfo(ec_sync_info_t **syncInfo){
    if(*syncInfo == NULL){
        ec_sync_info_t* syncInfoTemp = new ec_sync_info_t[3];
        syncInfoTemp[0].n_pdos = 0;
        syncInfoTemp[0].dir = EC_DIR_INVALID;
        syncInfoTemp[0].pdos = NULL;
        syncInfoTemp[1].n_pdos = 0;
        syncInfoTemp[1].dir = EC_DIR_INVALID;
        syncInfoTemp[1].pdos = NULL;

        syncInfoTemp[2].index = (uint8_t)EC_END;
        *syncInfo = syncInfoTemp;
    }

    CreatePdo(m_EtherCATInfo_, "RxPdo", &(*syncInfo)[0]);
    CreatePdo(m_EtherCATInfo_, "TxPdo", &(*syncInfo)[1]);
    return true;
}
